// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
 * Author: Bence Magyar, Enrique Fernández, Manuel Meraz
 */

#include <memory>
#include <queue>
#include <string>
#include <utility>
#include <vector>

#include "four_wheel_steering_controller1/four_wheel_steering_controller.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "tf2/LinearMath/Quaternion.h"

namespace {
    constexpr auto DEFAULT_COMMAND_TOPIC = "/four_wheel_steering_controller/cmd_vel";
    constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped";
    constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out";
    constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
    constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
}  // namespace

namespace four_wheel_steering_controller {
    using namespace std::chrono_literals;
    using controller_interface::interface_configuration_type;
    using controller_interface::InterfaceConfiguration;
    using hardware_interface::HW_IF_POSITION;
    using hardware_interface::HW_IF_VELOCITY;
    using lifecycle_msgs::msg::State;

    FourWheelSteeringController::FourWheelSteeringController() : controller_interface::ControllerInterface() {}

    // const char* FourWheelSteeringController::feedback_type() const {
    //     return params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY;
    // }

    controller_interface::CallbackReturn FourWheelSteeringController::on_init() {
        //设置logger 日志等级
        kLogger_ = get_node()->get_logger();
        kLogger_.set_level(rclcpp::Logger::Level::Debug);
        RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController::on_init() 初始化参数");

        try {
            // Create the parameter listener and get the parameters
            param_listener_ = std::make_shared<ParamListener>(get_node());
            params_ = param_listener_->get_params();
        }
        catch (const std::exception& e) {
            fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
            return controller_interface::CallbackReturn::ERROR;
        }
        results_publish_enable_ = params_.results_publish_enable;
        RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController::on_init() 初始化参数 end");
        return controller_interface::CallbackReturn::SUCCESS;
    }
    InterfaceConfiguration FourWheelSteeringController::command_interface_configuration() const {
        RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController::command_interface_configuration() ");

        std::vector<std::string> conf_names;

        if (results_publish_enable_) {
            RCLCPP_INFO(kLogger_, "FourWheelSteeringController::results_publish_enable_");
        }
        else {
            for (const auto& joint_name : params_.wheel_names) {
                conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY);
            }
            for (const auto& joint_name : params_.steering_names) {
                conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
            }
        }
        return { interface_configuration_type::INDIVIDUAL, conf_names };
    }

    InterfaceConfiguration FourWheelSteeringController::state_interface_configuration() const {
        RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController::state_interface_configuration() ");

        std::vector<std::string> conf_names;
        for (const auto& joint_name : params_.wheel_names) {
            conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY);
        }
        for (const auto& joint_name : params_.wheel_names) {
            conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
        }
        for (const auto& joint_name : params_.steering_names) {
            conf_names.push_back(joint_name + "/" + HW_IF_VELOCITY);
        }
        for (const auto& joint_name : params_.steering_names) {
            conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
        }
        return { interface_configuration_type::INDIVIDUAL, conf_names };
    }

    controller_interface::CallbackReturn FourWheelSteeringController::on_configure(
        const rclcpp_lifecycle::State&) {
        // auto kLogger_ = get_node()->get_logger();
        RCLCPP_INFO(kLogger_, "Configuring FourWheelSteeringController");
        // update parameters if they have changed
        if (param_listener_->is_old(params_)) {
            params_ = param_listener_->get_params();
            RCLCPP_INFO(kLogger_, "Parameters were updated");
        }

        // if (params_.left_wheel_names.size() != params_.right_wheel_names.size()) {
        //     RCLCPP_ERROR(
        //         kLogger_, "The number of left wheels [%zu] and the number of right wheels [%zu] are different",
        //         params_.left_wheel_names.size(), params_.right_wheel_names.size());
        //     return controller_interface::CallbackReturn::ERROR;
        // }

        // if (params_.left_wheel_names.empty()) {
        //     RCLCPP_ERROR(kLogger_, "Wheel names parameters are empty!");
        //     return controller_interface::CallbackReturn::ERROR;
        // }

        // const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
        // const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
        // const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;

                //轮距 //轮半径
        const double wheel_base = params_.wheel_base_multiplier * params_.wheel_base;
        const double wheel_track = params_.wheel_track_multiplier * params_.wheel_track;
        const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius;
        const double wheel_num = params_.wheel_num;

        twist2four_.conf(wheel_base, wheel_track, wheel_radius, 0.7, 0.7);
        // odometry_.setWheelParams(wheel_separation, left_wheel_radius, right_wheel_radius);
        odometry_.setWheelParams(wheel_base, wheel_track, wheel_radius, wheel_num);
        //设置平均值的累加个数
        odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size);

        cmd_vel_timeout_ = std::chrono::milliseconds{ static_cast<int>(params_.cmd_vel_timeout * 1000.0) };

        publish_limited_velocity_ = params_.publish_limited_velocity;

        use_stamped_vel_ = params_.use_stamped_vel;

        results_publish_enable_ = params_.results_publish_enable;
        RCLCPP_INFO(kLogger_, "       results_publish_enable_ = %d", results_publish_enable_);
        //轮
        if (params_.wheel_names.empty()) {
            RCLCPP_ERROR(kLogger_, "Wheel names parameters are empty!");
            return CallbackReturn::ERROR;
        }
        //转向
        if (params_.steering_names.empty()) {
            RCLCPP_ERROR(kLogger_, "steering names parameters are empty!");
            return CallbackReturn::ERROR;
        }


        limiter_x_linear_ = SpeedLimiter(
            params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits,
            params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity,
            params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk,
            params_.linear.x.max_jerk);

        limiter_y_linear_ = SpeedLimiter(
            params_.linear.y.has_velocity_limits, params_.linear.y.has_acceleration_limits,
            params_.linear.y.has_jerk_limits, params_.linear.y.min_velocity, params_.linear.y.max_velocity,
            params_.linear.y.min_acceleration, params_.linear.y.max_acceleration, params_.linear.y.min_jerk,
            params_.linear.y.max_jerk);

        limiter_angular_ = SpeedLimiter(
            params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits,
            params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity,
            params_.angular.z.max_velocity, params_.angular.z.min_acceleration,
            params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk);

        if (!reset()) {
            return controller_interface::CallbackReturn::ERROR;
        }

        // left and right sides are both equal at this point
        // params_.wheels_per_side = params_.left_wheel_names.size();

        if (publish_limited_velocity_) {
            limited_velocity_publisher_ =
                get_node()->create_publisher<Twist>(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
            realtime_limited_velocity_publisher_ =
                std::make_shared<realtime_tools::RealtimePublisher<Twist>>(limited_velocity_publisher_);
        }

        const Twist empty_twist;
        received_velocity_msg_ptr_.set(std::make_shared<Twist>(empty_twist));

        // Fill last two commands with default constructed commands
        previous_commands_.emplace(empty_twist);
        previous_commands_.emplace(empty_twist);

        // initialize command subscriber
        if (use_stamped_vel_) {
            velocity_command_subscriber_ = get_node()->create_subscription<Twist>(
                DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
                [this](const std::shared_ptr<Twist> msg) -> void {
                    if (!subscriber_is_active_) {
                        RCLCPP_WARN(
                            get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
                        return;
                    }
                    if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) {
                        RCLCPP_WARN_ONCE(
                            get_node()->get_logger(),
                            "Received TwistStamped with zero timestamp, setting it to current "
                            "time, this message will only be shown once");
                        msg->header.stamp = get_node()->get_clock()->now();
                    }
                    // RCLCPP_DEBUG(kLogger_, "vx %f ,vy %f ,vz %f ",msg->twist.linear.x,  msg->twist.linear.y,msg->twist.angular.z);
                    received_velocity_msg_ptr_.set(std::move(msg));
                });
        }
        else {
            velocity_command_unstamped_subscriber_ =
                get_node()->create_subscription<geometry_msgs::msg::Twist>(
                    DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(),
                    [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) -> void {
                        if (!subscriber_is_active_) {
                            RCLCPP_WARN(
                                get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
                            return;
                        }
                        // Write fake header in the stored stamped command
                        std::shared_ptr<Twist> twist_stamped;
                        received_velocity_msg_ptr_.get(twist_stamped);
                        twist_stamped->twist = *msg;
                        twist_stamped->header.stamp = get_node()->get_clock()->now();
                    });
        }

        // initialize odometry publisher and messasge
        odometry_publisher_ = get_node()->create_publisher<nav_msgs::msg::Odometry>(
            DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS());
        realtime_odometry_publisher_ =
            std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(
                odometry_publisher_);

        // Append the tf prefix if there is one
        std::string tf_prefix = "";
        if (params_.tf_frame_prefix_enable) {
            if (params_.tf_frame_prefix != "") {
                tf_prefix = params_.tf_frame_prefix;
            }
            else {
                tf_prefix = std::string(get_node()->get_namespace());
            }
            if (tf_prefix == "/") {
                tf_prefix = "";
            }
            else {
                tf_prefix = tf_prefix + "/";
            }
        }

        const auto odom_frame_id = tf_prefix + params_.odom_frame_id;
        const auto base_frame_id = tf_prefix + params_.base_frame_id;

        auto& odometry_message = realtime_odometry_publisher_->msg_;
        odometry_message.header.frame_id = odom_frame_id;
        odometry_message.child_frame_id = base_frame_id;

        // limit the publication on the topics /odom and /tf
        publish_rate_ = params_.publish_rate;
        publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);

        // initialize odom values zeros
        odometry_message.twist =
            geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);

        constexpr size_t NUM_DIMENSIONS = 6;
        for (size_t index = 0; index < 6; ++index) {
            // 0, 7, 14, 21, 28, 35
            const size_t diagonal_index = NUM_DIMENSIONS * index + index;
            odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
            odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
        }

        // initialize transform publisher and message
        odometry_transform_publisher_ = get_node()->create_publisher<tf2_msgs::msg::TFMessage>(
            DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS());
        realtime_odometry_transform_publisher_ =
            std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(
                odometry_transform_publisher_);

        // keeping track of odom and base_link transforms only
        auto& odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
        odometry_transform_message.transforms.resize(1);
        odometry_transform_message.transforms.front().header.frame_id = odom_frame_id;
        odometry_transform_message.transforms.front().child_frame_id = base_frame_id;

        if (results_publish_enable_) {
            wheel_velocity_publisher_ = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>
                (params_.wheel_publish_name, rclcpp::SystemDefaultsQoS());
            realtime_wheel_velocity_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(wheel_velocity_publisher_);
            realtime_wheel_velocity_publisher_->msg_.data.resize(4);

            steering_radians_publisher_ = get_node()->create_publisher<std_msgs::msg::Float64MultiArray>(params_.steering_publish_name, rclcpp::SystemDefaultsQoS());
            realtime_steering_radians_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>(steering_radians_publisher_);
            realtime_steering_radians_publisher_->msg_.data.resize(4);
        }
        previous_update_timestamp_ = get_node()->get_clock()->now();

        odom_reset_service_ = get_node()->create_service<std_srvs::srv::Trigger>("/odom_reset",
            std::bind(&FourWheelSteeringController::odom_reset, this, std::placeholders::_1, std::placeholders::_2));
        return controller_interface::CallbackReturn::SUCCESS;
    }

    controller_interface::CallbackReturn FourWheelSteeringController::on_activate(
        const rclcpp_lifecycle::State&) {
        RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController Activating");
        // const auto left_result =
        //     configure_side("left", params_.left_wheel_names, registered_left_wheel_handles_);
        // const auto right_result =
        //     configure_side("right", params_.right_wheel_names, registered_right_wheel_handles_);
        // if (results_publish_enable_) {

        // }
        // else {
        const auto wheel_result =
            get_wheel(params_.wheel_names, wheel_joint_);
        const auto steering_result =
            get_steering(params_.steering_names, steering_joint_);

        // if (
        //     left_result == controller_interface::CallbackReturn::ERROR ||
        //     right_result == controller_interface::CallbackReturn::ERROR) {
        //     return controller_interface::CallbackReturn::ERROR;
        // }
        if (wheel_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) {
            return CallbackReturn::ERROR;
        }

        // if (registered_left_wheel_handles_.empty() || registered_right_wheel_handles_.empty()) {
        //     RCLCPP_ERROR(
        //         get_node()->get_logger(),
        //         "Either left wheel interfaces, right wheel interfaces are non existent");
        //     return controller_interface::CallbackReturn::ERROR;
        // }
        if (wheel_joint_.empty() || steering_joint_.empty()) {
            RCLCPP_ERROR(
                kLogger_,
                "Either left wheel interfaces, right wheel interfaces are non existent");
            return CallbackReturn::ERROR;
        }
        // }
        is_halted = false;
        subscriber_is_active_ = true;

        RCLCPP_DEBUG(kLogger_, "Subscriber and publisher are now active.");
        return controller_interface::CallbackReturn::SUCCESS;
    }

    controller_interface::return_type FourWheelSteeringController::update(
        const rclcpp::Time& time, const rclcpp::Duration& period) {
        // RCLCPP_DEBUG(kLogger_, "FourWheelSteeringController::update() ");

        if (get_state().id() == State::PRIMARY_STATE_INACTIVE) {
            if (!is_halted) {
                halt();
                is_halted = true;
            }
            return controller_interface::return_type::OK;
        }

        std::shared_ptr<Twist> last_command_msg;
        received_velocity_msg_ptr_.get(last_command_msg);
        // RCLCPP_DEBUG(kLogger_, "vx %f ,vy %f ,vz %f ",last_command_msg->twist.linear.x,last_command_msg->twist.linear.y,last_command_msg->twist.angular.z);

        if (last_command_msg == nullptr) {
            RCLCPP_WARN(kLogger_, "Velocity message received was a nullptr.");
            return controller_interface::return_type::ERROR;
        }

        const auto age_of_last_command = time - last_command_msg->header.stamp;
        // Brake if cmd_vel has timeout, override the stored command
        if (age_of_last_command > cmd_vel_timeout_) {
            last_command_msg->twist.linear.x = 0.0;
            last_command_msg->twist.linear.y = 0.0;
            last_command_msg->twist.angular.z = 0.0;
        }

        // command may be limited further by SpeedLimit,
        // without affecting the stored twist command
        Twist command = *last_command_msg;
        double& linear_x_command = command.twist.linear.x;
        double& linear_y_command = command.twist.linear.y;
        double& angular_command = command.twist.angular.z;

        previous_update_timestamp_ = time;

        // Apply (possibly new) multipliers:
        // 应用(可能是新的)乘数:  params_有可能是动态获取参数
        // Wheel track => 轮距  Wheel base => 轴距 Wheel radius => 轮半径
        // const double wheel_track = params_.wheel_track_multiplier * params_.wheel_track;
        // const double wheel_base = params_.wheel_base_multiplier * params_.wheel_base;
        const double wheel_radius = params_.wheel_radius_multiplier * params_.wheel_radius;

        // const double steering_reduction = 50.0 * 118 / 18;
        // const double wheel_reduction = 88 * 99 / 18 / 14;
        // const double mps_to_rpm = 60 / 2 / M_PI / wheel_radius;
        // // 弧度转角度系数
        // const double rad_to_deg = 180.0 / M_PI;

        double wheel_state_feedback[4] = {
             wheel_joint_[0].feedback.get().get_value() * wheel_radius  ,
             wheel_joint_[1].feedback.get().get_value() * wheel_radius  ,
             wheel_joint_[2].feedback.get().get_value() * wheel_radius  ,
             wheel_joint_[3].feedback.get().get_value() * wheel_radius };
        //          wheel_joint_[0].feedback.get().get_value() / mps_to_rpm / wheel_reduction ,
        //          wheel_joint_[1].feedback.get().get_value() / mps_to_rpm / wheel_reduction ,
        //          wheel_joint_[2].feedback.get().get_value() / mps_to_rpm / wheel_reduction ,
        //              wheel_joint_[3].feedback.get().get_value() / mps_to_rpm / wheel_reduction
        // };
        double steering_state_feedback[4] = {
       steering_joint_[0].feedback.get().get_value() ,
        steering_joint_[1].feedback.get().get_value(),
        steering_joint_[2].feedback.get().get_value(),
        steering_joint_[3].feedback.get().get_value() };
        // RCLCPP_DEBUG(kLogger_,"wheel_state_feedback %f ,%f ,%f ,%f",wheel_state_feedback[0],wheel_state_feedback[1],wheel_state_feedback[2],wheel_state_feedback[3]);
        // RCLCPP_DEBUG(kLogger_,"steering_state_feedback %f ,%f ,%f ,%f",steering_state_feedback[0],steering_state_feedback[1],steering_state_feedback[2],steering_state_feedback[3]);

        //              steering_joint_[0].feedback.get().get_value() / steering_reduction,
        //                  steering_joint_[1].feedback.get().get_value() / steering_reduction,
        //                  steering_joint_[2].feedback.get().get_value() / steering_reduction,
        //                  steering_joint_[3].feedback.get().get_value() / steering_reduction
        // };


            // // Apply (possibly new) multipliers:
            // const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
            // const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
            // const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;

        if (params_.open_loop) {
            odometry_.updateOpenLoop(linear_x_command, angular_command, time);
        }
        else {
            // RCLCPP_DEBUG(kLogger_, "wheel_state_feedback=%f,%f,%f,%f,steering_state_feedback=%f,%f,%f,%f,", wheel_state_feedback[0], wheel_state_feedback[1], wheel_state_feedback[2], wheel_state_feedback[3], steering_state_feedback[0], steering_state_feedback[1], steering_state_feedback[2], steering_state_feedback[3]);
            odometry_.updateFromVelocity(wheel_state_feedback, steering_state_feedback, time);
            // RCLCPP_DEBUG(kLogger_, "heading=%f", odometry_.getHeading());
        }

        tf2::Quaternion orientation;
        orientation.setRPY(0.0, 0.0, odometry_.getHeading());

        bool should_publish = false;
        try {
            if (previous_publish_timestamp_ + publish_period_ < time) {
                previous_publish_timestamp_ += publish_period_;
                should_publish = true;
            }
        }
        catch (const std::runtime_error&) {
            // Handle exceptions when the time source changes and initialize publish timestamp
            previous_publish_timestamp_ = time;
            should_publish = true;
        }

        if (should_publish) {
            if (realtime_odometry_publisher_->trylock()) {
                auto& odometry_message = realtime_odometry_publisher_->msg_;
                odometry_message.header.stamp = time;
                odometry_message.pose.pose.position.x = odometry_.getX();
                odometry_message.pose.pose.position.y = odometry_.getY();
                odometry_message.pose.pose.orientation.x = orientation.x();
                odometry_message.pose.pose.orientation.y = orientation.y();
                odometry_message.pose.pose.orientation.z = orientation.z();
                odometry_message.pose.pose.orientation.w = orientation.w();
                odometry_message.twist.twist.linear.x = odometry_.getLinearX();
                odometry_message.twist.twist.linear.y = odometry_.getLinearY();
                odometry_message.twist.twist.angular.z = odometry_.getAngular();
                realtime_odometry_publisher_->unlockAndPublish();
            }

            if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) {
                auto& transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
                transform.header.stamp = time;
                transform.transform.translation.x = odometry_.getX();
                transform.transform.translation.y = odometry_.getY();
                transform.transform.rotation.x = orientation.x();
                transform.transform.rotation.y = orientation.y();
                transform.transform.rotation.z = orientation.z();
                transform.transform.rotation.w = orientation.w();
                realtime_odometry_transform_publisher_->unlockAndPublish();
            }
        }

        auto& last_command = previous_commands_.back().twist;
        auto& second_to_last_command = previous_commands_.front().twist;
        if (linear_x_command > 0) {
            limiter_x_linear_.set_acceleration_limits(params_.linear.x.min_acceleration, params_.linear.x.max_acceleration);
        }
        else if (linear_x_command < 0) {
            limiter_x_linear_.set_acceleration_limits(params_.linear.x.max_acceleration * -1, params_.linear.x.min_acceleration * -1);
        }
        limiter_x_linear_.limit(
            linear_x_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds());
        if (linear_y_command > 0) {
            limiter_y_linear_.set_acceleration_limits(params_.linear.y.min_acceleration, params_.linear.y.max_acceleration);
        }
        else if (linear_y_command < 0) {
            limiter_y_linear_.set_acceleration_limits(params_.linear.y.max_acceleration * -1, params_.linear.y.min_acceleration * -1);
        }
        limiter_y_linear_.limit(
            linear_y_command, last_command.linear.y, second_to_last_command.linear.y, period.seconds());
        if (angular_command > 0) {
            limiter_angular_.set_acceleration_limits(params_.angular.z.min_acceleration, params_.angular.z.max_acceleration);
        }
        else if (angular_command < 0) {
            limiter_angular_.set_acceleration_limits(params_.angular.z.max_acceleration  * -1, params_.angular.z.min_acceleration * -1);
        }
        limiter_angular_.limit(
            angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds());

        previous_commands_.pop();
        previous_commands_.emplace(command);

        //    Publish limited velocity
        if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) {
            auto& limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
            limited_velocity_command.header.stamp = time;
            limited_velocity_command.twist = command.twist;
            realtime_limited_velocity_publisher_->unlockAndPublish();
        }
        /*
                // Compute wheels velocities:
                // const double velocity_left =
                //     (linear_command - angular_command * wheel_separation / 2.0) / left_wheel_radius;
                // const double velocity_right =
                //     (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius;

                //     double wheel_track, wheel_base, wheel_radius;
                //     //
                //   //front_left         第一象限 (x:wheel_base/2,y:wheel_track/2)
                //   //front_right      第四象限  (x:wheel_base/2,y:-wheel_track/2)
                //   //rear_left          第二象限  (x:-wheel_base/2,y:wheel_track/2)
                //   //rear_right        第三象限  (x:-wheel_base/2,y:-wheel_track/2)
                //     //四轮转向算法
                //     double v1x, v1y, v2x, v2y, v3x, v3y, v4x, v4y;
                //     double linearx, lineary, angularz;

                // v1x = linearx - angularz * wheel_track / 2.0;
                // v1y = lineary + angularz * wheel_base / 2.0;
                // v2x = linearx - angularz * wheel_track / 2.0;
                // v2y = lineary - angularz * wheel_base / 2.0;
                // v3x = linearx + angularz * wheel_track / 2.0;
                // v3y = lineary - angularz * wheel_base / 2.0;
                // v4x = linearx + angularz * wheel_track / 2.0;
                // v4y = lineary + angularz * wheel_base / 2.0;

                // double front_left_wheel_velocity
                //     = sqrt(v1x * v1x + v1y * v1y) / wheel_radius;
                // double rear_left_wheel_velocity
                //     = sqrt(v2x * v2x + v2y * v2y) / wheel_radius;
                // double rear_right_wheel_velocity
                //     = sqrt(v3x * v3x + v3y * v3y) / wheel_radius;
                // double front_right_wheel_velocity
                //     = sqrt(v4x * v4x + v4y * v4y) / wheel_radius;
                // double front_left_steering
                //     = atan2(v1y, v1x);
                // double rear_left_steering
                //     = atan2(v2y, v2x);
                // double rear_right_steering
                //     = atan2(v3y, v3x);
                // double front_right_steering
                //     = atan2(v4y, v4x);

                // if (front_left_steering < -M_PI / 2) {
                //     front_left_steering = front_left_steering + M_PI;
                //     front_left_wheel_velocity = -front_left_wheel_velocity;
                // }
                // else  if (front_left_steering > M_PI / 2) {
                //     front_left_steering = front_left_steering - M_PI;
                //     front_left_wheel_velocity = -front_left_wheel_velocity;
                // }
                // if (rear_left_steering < -M_PI / 2) {
                //     rear_left_steering = rear_left_steering + M_PI;
                //     rear_left_wheel_velocity = -rear_left_wheel_velocity;
                // }
                // else  if (rear_left_steering > M_PI / 2) {
                //     rear_left_steering = rear_left_steering - M_PI;
                //     rear_left_wheel_velocity = -rear_left_wheel_velocity;
                // }
                // if (rear_right_steering < -M_PI / 2) {
                //     rear_right_steering = rear_right_steering + M_PI;
                //     rear_right_wheel_velocity = -rear_right_wheel_velocity;
                // }
                // else  if (rear_right_steering > M_PI / 2) {
                //     rear_right_steering = rear_right_steering - M_PI;
                //     rear_right_wheel_velocity = -rear_right_wheel_velocity;
                // }
                // if (front_right_steering < -M_PI / 2) {
                //     front_right_steering = front_right_steering + M_PI;
                //     front_right_wheel_velocity = -front_right_wheel_velocity;
                // }
                // else  if (front_right_steering > M_PI / 2) {
                //     front_right_steering = front_right_steering - M_PI;
                //     front_right_wheel_velocity = -front_right_wheel_velocity;
                // }
                // if (max({ front_left_wheel_velocity,rear_left_wheel_velocity,rear_right_wheel_velocity,front_right_wheel_velocity }) > max_wheel_velocity_) {
                //     double max_velocity = max({ front_left_wheel_velocity,rear_left_wheel_velocity,rear_right_wheel_velocity,front_right_wheel_velocity });
                //     front_left_wheel_velocity = max_wheel_velocity_ * front_left_wheel_velocity / max_velocity;
                //     rear_left_wheel_velocity = max_wheel_velocity_ * rear_left_wheel_velocity / max_velocity;
                //     rear_right_wheel_velocity = max_wheel_velocity_ * rear_right_wheel_velocity / max_velocity;
                //     front_right_wheel_velocity = max_wheel_velocity_ * front_right_wheel_velocity / max_velocity;
                // }
                // if(min({ front_left_wheel_velocity,rear_left_wheel_velocity,rear_right_wheel_velocity,front_right_wheel_velocity }) < -max_wheel_velocity_
                // )
                // {
                //     double min_velocity = min({ front_left_wheel_velocity,rear_left_wheel_velocity,rear_right_wheel_velocity,front_right_wheel_velocity }
                //     );
                //     front_left_wheel_velocity = -max_wheel_velocity_ * front_left_wheel_velocity / min_velocity;
                //     rear_left_wheel_velocity = -max_wheel_velocity_ * rear_left_wheel_velocity / min_velocity;
                //     rear_right_wheel_velocity = -max_wheel_velocity_ * rear_right_wheel_velocity / min_velocity;
                //     front_right_wheel_velocity = -max_wheel_velocity_ * front_right_wheel_velocity / min_velocity;
                // }

        */
        // 发布速度指令
        twist2four_.four_wheel_steering_derivation(command, wheel_velocity_value, steering_radians_value);

        if(wheel_velocity_value[0]!=0){
            RCLCPP_INFO(kLogger_,"wheel_velocity_value %f ,%f ,%f ,%f",wheel_velocity_value[0],wheel_velocity_value[1],wheel_velocity_value[2],wheel_velocity_value[3]);
            RCLCPP_INFO(kLogger_,"steering_radians_value %f ,%f ,%f ,%f",steering_radians_value[0],steering_radians_value[1],steering_radians_value[2],steering_radians_value[3]);
        }
        // Set wheels velocities:
        // for (size_t index = 0; index < static_cast<size_t>(params_.wheels_per_side); ++index) {
        //     registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left);
        //     registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right);
        // }
        if (results_publish_enable_) {
            if (realtime_wheel_velocity_publisher_->trylock()) {
                auto& wheel_velocity_msg = realtime_wheel_velocity_publisher_->msg_;
                // wheel_velocity_msg.header.stamp = time;
                wheel_velocity_msg.data[0] = wheel_velocity_value[0] / wheel_radius;
                wheel_velocity_msg.data[1] = wheel_velocity_value[1] / wheel_radius;
                wheel_velocity_msg.data[2] = wheel_velocity_value[2] / wheel_radius;
                wheel_velocity_msg.data[3] = wheel_velocity_value[3] / wheel_radius;
                realtime_wheel_velocity_publisher_->unlockAndPublish();
            }
            if (realtime_steering_radians_publisher_->trylock()) {
                auto& steering_radians_msg = realtime_steering_radians_publisher_->msg_;
                // steering_msg.header.stamp = time;
                steering_radians_msg.data[0] = steering_radians_value[0];
                steering_radians_msg.data[1] = steering_radians_value[1];
                steering_radians_msg.data[2] = steering_radians_value[2];
                steering_radians_msg.data[3] = steering_radians_value[3];
                realtime_steering_radians_publisher_->unlockAndPublish();
            }
        }
        else {
            // Set wheels velocities:
            for (int64_t index = 0; index < 4; ++index) {
                // wheel_velocity_value[index] = command_four_each.wheel_velocity[index];
                // steering_position_value[index] = command_four_each.steering_position[index];
                // wheel_joint_[index].velocity.get().set_value(wheel_velocity_value[index]/ wheel_radius);
                steering_joint_[index].position.get().set_value(steering_radians_value[index]);
            }
            double temp_max=std::max({std::abs(steering_state_feedback[0]-steering_joint_[0].position.get().get_value()),
                                    std::abs(steering_state_feedback[1]-steering_joint_[1].position.get().get_value()),
                                    std::abs(steering_state_feedback[2]-steering_joint_[2].position.get().get_value()),
                                    std::abs(steering_state_feedback[3]-steering_joint_[3].position.get().get_value())});

            if(temp_max<0.5){
                wheel_joint_[0].velocity.get().set_value(wheel_velocity_value[0]/ wheel_radius*(0.5-temp_max)/0.5);
                wheel_joint_[1].velocity.get().set_value(wheel_velocity_value[1]/ wheel_radius*(0.5-temp_max)/0.5);
                wheel_joint_[2].velocity.get().set_value(wheel_velocity_value[2]/ wheel_radius*(0.5-temp_max)/0.5);
                wheel_joint_[3].velocity.get().set_value(wheel_velocity_value[3]/ wheel_radius*(0.5-temp_max)/0.5);
            }
            else{
                wheel_joint_[0].velocity.get().set_value(0);
                wheel_joint_[1].velocity.get().set_value(0);
                wheel_joint_[2].velocity.get().set_value(0);
                wheel_joint_[3].velocity.get().set_value(0);
                wheel_velocity_value[0] = 0;
                wheel_velocity_value[1] = 0;
                wheel_velocity_value[2] = 0;
                wheel_velocity_value[3] = 0;
            }
            //             if((std::abs(steering_state_feedback[0]-steering_joint_[0].position.get().get_value())<0.1)
            // ||(std::abs(steering_state_feedback[1]-steering_joint_[1].position.get().get_value())<0.1)
            // ||(std::abs(steering_state_feedback[2]-steering_joint_[2].position.get().get_value())<0.1)
            // ||(std::abs(steering_state_feedback[3]-steering_joint_[3].position.get().get_value())<0.1)){
            //     wheel_joint_[0].velocity.get().set_value(wheel_velocity_value[0]/ wheel_radius);
            //     wheel_joint_[1].velocity.get().set_value(wheel_velocity_value[1]/ wheel_radius);
            //     wheel_joint_[2].velocity.get().set_value(wheel_velocity_value[2]/ wheel_radius);
            //     wheel_joint_[3].velocity.get().set_value(wheel_velocity_value[3]/ wheel_radius);
            // }
            // else{
            //     wheel_joint_[0].velocity.get().set_value(0);
            //     wheel_joint_[1].velocity.get().set_value(0);
            //     wheel_joint_[2].velocity.get().set_value(0);
            //     wheel_joint_[3].velocity.get().set_value(0);
            //     wheel_velocity_value[0] = 0;
            //     wheel_velocity_value[1] = 0;
            //     wheel_velocity_value[2] = 0;
            //     wheel_velocity_value[3] = 0;
            // }
        }


        return controller_interface::return_type::OK;
    }
    controller_interface::CallbackReturn FourWheelSteeringController::on_deactivate(
        const rclcpp_lifecycle::State&) {
        subscriber_is_active_ = false;
        if (!is_halted) {
            halt();
            is_halted = true;
        }
        wheel_joint_.clear();
        steering_joint_.clear();
        return controller_interface::CallbackReturn::SUCCESS;
    }

    controller_interface::CallbackReturn FourWheelSteeringController::on_cleanup(
        const rclcpp_lifecycle::State&) {
        if (!reset()) {
            return controller_interface::CallbackReturn::ERROR;
        }

        received_velocity_msg_ptr_.set(std::make_shared<Twist>());
        return controller_interface::CallbackReturn::SUCCESS;
    }

    controller_interface::CallbackReturn FourWheelSteeringController::on_error(const rclcpp_lifecycle::State&) {
        if (!reset()) {
            return controller_interface::CallbackReturn::ERROR;
        }
        return controller_interface::CallbackReturn::SUCCESS;
    }

    bool FourWheelSteeringController::reset() {
        odometry_.resetOdometry();

        //清空关节
        wheel_joint_.clear();
        steering_joint_.clear();

        // release the old queue
        std::queue<Twist> empty;
        std::swap(previous_commands_, empty);

        // registered_left_wheel_handles_.clear();
        // registered_right_wheel_handles_.clear();

        subscriber_is_active_ = false;
        velocity_command_subscriber_.reset();
        velocity_command_unstamped_subscriber_.reset();

        received_velocity_msg_ptr_.set(nullptr);
        is_halted = false;
        return true;
    }

    controller_interface::CallbackReturn FourWheelSteeringController::on_shutdown(
        const rclcpp_lifecycle::State&) {
        return controller_interface::CallbackReturn::SUCCESS;
    }

    void FourWheelSteeringController::halt() {
        if (results_publish_enable_) {
        }
        else {
            for (const auto& wheel_handle : wheel_joint_) {
                wheel_handle.velocity.get().set_value(0.0);
            }
            for (const auto& wheel_handle : wheel_joint_) {
                wheel_handle.velocity.get().set_value(0.0);
            }
        }
    }
    controller_interface::CallbackReturn FourWheelSteeringController::get_wheel(
        const std::vector<std::string>& wheel_names,
        std::vector<WheelHandle>& registered_joint) {
        auto kLogger_ = get_node()->get_logger();

        if (wheel_names.empty()) {
            RCLCPP_ERROR(kLogger_, "No wheel names specified");
            return CallbackReturn::ERROR;
        }

        // register joint
        registered_joint.reserve(wheel_names.size());
        for (const auto& wheel_name : wheel_names) {
            // const auto interface_name = HW_IF_VELOCITY;
            const auto state_handle = std::find_if(
                state_interfaces_.cbegin(), state_interfaces_.cend(),
                [&wheel_name](const auto& interface) {
                    return interface.get_prefix_name() == wheel_name &&
                        interface.get_interface_name() == HW_IF_VELOCITY;
                });

            if (state_handle == state_interfaces_.cend()) {
                RCLCPP_ERROR(kLogger_, "Unable to obtain joint state handle for %s", wheel_name.c_str());
                return CallbackReturn::ERROR;
            }


            const auto command_handle = std::find_if(
                command_interfaces_.begin(), command_interfaces_.end(),
                [&wheel_name](const auto& interface) {
                    return interface.get_prefix_name() == wheel_name &&
                        interface.get_interface_name() == HW_IF_VELOCITY;
                });

            if (!results_publish_enable_) {
                if (command_handle == command_interfaces_.end()) {
                    RCLCPP_ERROR(kLogger_, "Unable to obtain joint command handle for %s", wheel_name.c_str());
                    return CallbackReturn::ERROR;
                }
            }

            registered_joint.emplace_back(
                WheelHandle{ std::ref(*state_handle), std::ref(*command_handle) });

        }

        return CallbackReturn::SUCCESS;
    }

    controller_interface::CallbackReturn FourWheelSteeringController::get_steering(
        const std::vector<std::string>& wheel_names,
        std::vector<SteeringHandle>& registered_joint) {
        auto kLogger_ = get_node()->get_logger();

        if (wheel_names.empty()) {
            RCLCPP_ERROR(kLogger_, "No  wheel names specified");
            return CallbackReturn::ERROR;
        }
        registered_joint.reserve(wheel_names.size());
        for (const auto& wheel_name : wheel_names) {
            const auto state_handle = std::find_if(
                state_interfaces_.cbegin(), state_interfaces_.cend(),
                [&wheel_name](const auto& interface) {
                    return interface.get_prefix_name() == wheel_name &&
                        interface.get_interface_name() == HW_IF_POSITION;
                });
            //没找到
            if (state_handle == state_interfaces_.cend()) {
                RCLCPP_ERROR(kLogger_, "Unable to obtain joint state handle for %s", wheel_name.c_str());
                return CallbackReturn::ERROR;
            }

            //找到一个硬件命令接口
            const auto command_handle = std::find_if(
                command_interfaces_.begin(), command_interfaces_.end(),
                [&wheel_name](const auto& interface) {
                    return interface.get_prefix_name() == wheel_name &&
                        interface.get_interface_name() == HW_IF_POSITION;
                });
            if (!results_publish_enable_) {
                //没找到
                if (command_handle == command_interfaces_.end()) {
                    RCLCPP_ERROR(kLogger_, "Unable to obtain joint command handle for %s", wheel_name.c_str());
                    return CallbackReturn::ERROR;
                }
            }
            //填充硬件接口
            registered_joint.emplace_back(
                SteeringHandle{ std::ref(*state_handle), std::ref(*command_handle) });
        }
        return CallbackReturn::SUCCESS;
    }

    void FourWheelSteeringController::odom_reset(const std::shared_ptr<std_srvs::srv::Trigger::Request> /*request*/,
        std::shared_ptr<std_srvs::srv::Trigger::Response>      response) {

        // if (request->data == true)
        // {

        odometry_.resetOdometry();
        RCLCPP_INFO(kLogger_, "Unable");
        // }
        response->success = true;

    }
}  // namespace four_wheel_steering_controller

#include "class_loader/register_macro.hpp"

CLASS_LOADER_REGISTER_CLASS(
    four_wheel_steering_controller::FourWheelSteeringController, controller_interface::ControllerInterface)
